﻿#pragma once
#pragma warning(disable:4251)
#pragma warning(disable:4506)
/******************************************************************************
  @Project Name  : 03TestFCLCollision
  @File Name     : Object.h
  @Version       : 1.0
  @Function      : to be determined
  @Author        : unishuai
  @Date          : 2024-05-20
******************************************************************************/

#include <Eigen/Dense>

#include <QtXml>

#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include <array>

#include <fcl/narrowphase/distance.h>
#include <fcl/narrowphase/distance_request.h>
#include <fcl/narrowphase/distance_result.h>
//定义几何形状：首先，需要定义你要检测碰撞的几何形状。FCL 支持各种形状，如盒子、球体，以及更复杂的模型，如 BVH (Bounding Volume Hierarchies)。

//创建碰撞对象：使用这些形状来创建碰撞对象。FCL 中的碰撞对象包括几何体（形状）及其姿态（位置和方向）。

//设置碰撞管理器：你可以选择使用碰撞管理器来有效处理多个碰撞对象。

//进行碰撞检测：最后，执行碰撞检测查询。这可以是简单的重叠检查、距离计算或更复杂的连续碰撞检测。

//typedef fcl::BVHModel<fcl::RSSd> Model;
typedef fcl::BVHModel<fcl::OBBRSSd> Model;

namespace RobotBasic
{
    const double PI = 3.14159265358979323846;

    inline Eigen::Matrix4d PE2T(double x,double y,double z,double rx,double ry,double rz)
    {
        double a = rx * RobotBasic::PI / 180;
        double b = ry * RobotBasic::PI / 180;
        double c = rz * RobotBasic::PI / 180;
        Eigen::Matrix4d trans;
        //捏麻麻的，这个也是牛逼，还可以这么重载输出运算符是吧。。。。
        //这个是初始位置的变换矩阵
        /*
         * 1 0 0 x
         * 0 1 0 y
         * 0 0 1 z
         * 0 0 0 1
         */
        trans << 1, 0, 0, x, 0, 1, 0, y, 0, 0, 1, z, 0, 0, 0, 1;
        Eigen::Matrix4d rotx;
        /*
         * 1   0      0     0
         * 0 cos(a) -sin(a) 0
         * 0 sin(a) cos(a)  0
         * 0   0      0     1
         */
        rotx << 1, 0, 0, 0, 0, cos(a), (-1)*sin(a), 0, 0, sin(a), cos(a), 0, 0, 0, 0, 1;
        Eigen::Matrix4d roty;
        /*
         * cos(b)     0     sin(b)    0
         * 0          1     0         0
         * -sin(b)    0     cos(b)    0
         * 0          0      0        1
         */
        roty << cos(b), 0, sin(b), 0, 0, 1, 0, 0, (-1)*sin(b), 0, cos(b), 0, 0, 0, 0, 1;
        Eigen::Matrix4d rotz;
        /*
         * cos(c)  -sin(c)  0    0
         * sin(c)  cos(c)   0    0
         *  0      0        1    0
         *  0      0        0    1
         */
        rotz << cos(c), (-1)*sin(c), 0, 0, sin(c), cos(c), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;

        Eigen::Matrix4d T;
        T = trans * rotz * roty * rotx;
        return T;
    }

    inline Eigen::Matrix4d RZ(double rz)
    {
        return PE2T(0,0,0,0,0,rz);
    }
    inline Eigen::Matrix4d PX(double px)
    {
        return PE2T(px,0,0,0,0,0);
    }
}


class Object
{
public:
	Object(std::string name);
    virtual ~Object(){}

    std::string m_Name;
};


class RobotObj: public Object
{
public:
    RobotObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT);
    ~RobotObj() = default;
//    表示机器人的自由度数量。
    int m_Dof;
    //存储机器人基座的一系列变换矩阵
    std::vector<Eigen::Matrix4d> m_Base;
    //一个 fcl::CollisionObjectd 类型的向量，用于碰撞检测。这些对象可能与机器人的每个部分相关联。
    std::vector<fcl::CollisionObjectd > m_ColliObj;
    //m_TransIiNow是相对于世界坐标系
    //m_PartT这个是相对于上一个坐标系

    //存储机器人各部件当前的变换矩阵。
    std::vector<Eigen::Matrix4d> m_PartT;
    //存储当前时刻机器人各部件的变换矩阵
    std::vector<Eigen::Matrix4d> m_TransIiNow;

    void updateStatus(Eigen::VectorXd jointangle,int base_index);
    void updateStatus(Eigen::VectorXd jointangle,Eigen::Matrix4d T_install);
    void updateStatusBySliderAndAngle(Eigen::VectorXd jointangle,int base_index);
    void calcTransIiNowBySliderAndAngle(Eigen::VectorXd jointangle,int base_index);
    void calcTransijNowBySliderAndAngle(Eigen::VectorXd &jntRad);
    void calcTransChangeBySliderAndAngle(Eigen::VectorXd &jntRad, std::vector<Eigen::Matrix4d> &transChange);

private:
    //存储与机器人各部件相关的几何模型。每个部件的模型可能用于计算机器人的几何形状、碰撞检测等。
    std::vector<std::shared_ptr<Model>> m_Model;
    //存储机器人各部件的初始变换矩阵。这些矩阵可能代表机器人部件在某个参考状态（如初始状态或“零”状态）下的位置和方向。
    std::vector<Eigen::Matrix4d> m_PartT_ini;

    void calcTransIiNow(Eigen::VectorXd &jntRad,int base_index);
    void calcTransIiNow(Eigen::VectorXd &jntRad,Eigen::Matrix4d T_install);
    void calcTransijNow(Eigen::VectorXd &jntRad);
    void calcTransChange(Eigen::VectorXd &jntRad, std::vector<Eigen::Matrix4d> &transChange);
    void setMColliObj();
};


class MotionPlatformObj: public Object
{
public:
    MotionPlatformObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT);
    ~MotionPlatformObj() = default;
//    表示机器人的自由度数量。
    int m_Dof;
    //存储机器人基座的一系列变换矩阵
    std::vector<Eigen::Matrix4d> m_Base;
    //一个 fcl::CollisionObjectd 类型的向量，用于碰撞检测。这些对象可能与机器人的每个部分相关联。
    std::vector<fcl::CollisionObjectd > m_ColliObj;
    //m_TransIiNow是相对于世界坐标系
    //m_PartT这个是相对于上一个坐标系

    //存储机器人各部件当前的变换矩阵。
    std::vector<Eigen::Matrix4d> m_PartT;
    //存储当前时刻机器人各部件的变换矩阵
    std::vector<Eigen::Matrix4d> m_TransIiNow;

    void updateStatus(Eigen::VectorXd jointangle,int base_index);
    void updateStatus(Eigen::VectorXd jointangle,Eigen::Matrix4d T_install);
    void updateStatusByDofAndRot(Eigen::VectorXd baseDof,Eigen::VectorXd jointangle,int base_index);


private:
    //存储与机器人各部件相关的几何模型。每个部件的模型可能用于计算机器人的几何形状、碰撞检测等。
    std::vector<std::shared_ptr<Model>> m_Model;
    //存储机器人各部件的初始变换矩阵。这些矩阵可能代表机器人部件在某个参考状态（如初始状态或“零”状态）下的位置和方向。
    std::vector<Eigen::Matrix4d> m_PartT_ini;

    void calcTransIiNow(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad,int base_index);
    void calcTransIiNow(Eigen::VectorXd &jntRad,Eigen::Matrix4d T_install);
    void calcTransijNow(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad);
    void calcTransChange(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad, std::vector<Eigen::Matrix4d> &transChange);
    void setMColliObj();
};




class TunnelWallObj: public Object
{
public:
    TunnelWallObj(std::string name, std::shared_ptr<Model> model, Eigen::Matrix4d partT);
    virtual ~TunnelWallObj() = default;
    //用于碰撞检测的 fcl::CollisionObjectd 对象。
    fcl::CollisionObjectd m_ColliObj;

    void updateStatus();
    void updateStatus(double angle);
    void setStatus(Eigen::Matrix4d trans);
    //Eigen::Matrix4d getMPartT();
    Eigen::Matrix4d getTransformMatrix() const;
    void setTransformMatrix(const Eigen::Matrix4d &PartT);
    void setTransformMatrixByPosAndRotVector(std::array<double,6> pose);

private:
    //用于表示动平台或者静平台的几何模型。
    std::shared_ptr<Model> m_Model;
    //平台的当前变换矩阵。
    Eigen::Matrix4d m_PartT;
};

class FlightObj: public Object
{
public:
    FlightObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT);

    virtual ~FlightObj() = default;

    int m_Dof;
    //存储机器人基座的一系列变换矩阵
    std::vector<Eigen::Matrix4d> m_Base;
    //一个 fcl::CollisionObjectd 类型的向量，用于碰撞检测。这些对象可能与机器人的每个部分相关联。
    //用于碰撞检测的 fcl::CollisionObjectd 对象。
    std::vector<fcl::CollisionObjectd > m_ColliObj;
    //m_TransIiNow是相对于世界坐标系
    //m_PartT这个是相对于上一个坐标系

    //存储机器人各部件当前的变换矩阵。
    std::vector<Eigen::Matrix4d> m_PartT;
    //存储当前时刻机器人各部件的变换矩阵
    std::vector<Eigen::Matrix4d> m_TransIiNow;

    void updateStatus(Eigen::VectorXd jointangle,int base_index);
    void updateStatus(Eigen::VectorXd jointangle,Eigen::Matrix4d T_install);

private:
    //存储与机器人各部件相关的几何模型。每个部件的模型可能用于计算机器人的几何形状、碰撞检测等。
    std::vector<std::shared_ptr<Model>> m_Model;
    //存储机器人各部件的初始变换矩阵。这些矩阵可能代表机器人部件在某个参考状态（如初始状态或“零”状态）下的位置和方向。
    std::vector<Eigen::Matrix4d> m_PartT_ini;

    void calcTransIiNow(Eigen::VectorXd &jntRad,int base_index);
    void calcTransIiNow(Eigen::VectorXd &jntRad,Eigen::Matrix4d T_install);
    void calcTransijNow(Eigen::VectorXd &jntRad);
    void calcTransChange(Eigen::VectorXd &jntRad, std::vector<Eigen::Matrix4d> &transChange);
    void setMColliObj();
};



class RadarObj: public Object
{
public:
    RadarObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> trans);
    ~RadarObj()=default;
    //雷达的自由度数。
    int m_dof;

    //存储雷达各部件的几何模型。
    const std::vector<std::shared_ptr<Model>> m_Model;

    //存储雷达各部件相对于初始位置的变换矩阵。
    const std::vector<Eigen::Matrix4d> m_IniRelTrans;

    // 存储雷达各部件当前的绝对变换矩阵。
    std::vector<Eigen::Matrix4d> m_NowAbsTrans;

    //用于雷达各部件的碰撞检测对象。
    std::vector<fcl::CollisionObjectd > m_ColliObj;

    //
    void updateStatus(Eigen::VectorXd &jntRad);
    void calcAbsTrans(Eigen::VectorXd &jntRad);
    void setMColliObj();
};
